package diffteleop_base;

import april.jmat.LinAlg;
import april.vis.VisCanvas;
import april.vis.VisChain;
import april.vis.VisLayer;
import april.vis.VisWorld;
import april.vis.VisWorld.Buffer;
import april.vis.VzBox;
import april.vis.VzCylinder;
import april.vis.VzGrid;
import april.vis.VzMesh;
import arlpanda.lcmtypes.simplepose_t;
import java.awt.BorderLayout;
import java.awt.Color;
import java.io.IOException;
import java.util.logging.Level;
import java.util.logging.Logger;
import javax.swing.JFrame;
import lcm.lcm.LCM;
import lcm.lcm.LCMDataInputStream;
import lcm.lcm.LCMSubscriber;

public class RobotGUI implements LCMSubscriber{

    //Dimensions in mm
    private static final double ROBOT_WIDTH = 15.5;
    private static final double ROBOT_HEIGHT = 7.5;
    private static final double ROBOT_DEPTH = 16.5;
    private static final double WHEEL_RADIUS = 4;
    private static final double WHEEL_WIDTH = 1;
    private static final double WHEEL_HEIGHT_OFFSET = 3;
    private static final double WHEEL_DEPTH_OFFSET = 6;
    private static final double FRONT_OFFSET = 4;
    
    //GUI Elements
    private final JFrame jFrame;
    private final VisWorld visWorld = new VisWorld();
    private final VisLayer visLayer = new VisLayer(visWorld);
    private final VisCanvas visCanvas = new VisCanvas(visLayer);

    //Robot State
    private double x = 0;
    private double y = 0;
    private double theta = 0;

    //Members
    private final LCM lcm;

    /**
     * Initializes the GUI and draws the robot at teh origin
     */
    public RobotGUI(LCM lcm) {
        this.lcm = lcm;
        lcm.subscribe("Team10_POSE", this);

        jFrame = new JFrame("Robot GUI");
        jFrame.setSize(1200, 800);
        jFrame.setVisible(true);
        jFrame.setLayout(new BorderLayout());
        jFrame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
        jFrame.add(visCanvas, BorderLayout.CENTER);

        updateRobot(0, 0, 0);
    }

    /**
     * Updates the position and heading of the robot
     * @param xPos
     * The current x position of the bot
     * @param yPos
     * The current y position of the bot
     * @param heading
     * The current heading of the bot
     */
    public void updateRobot(double xPos, double yPos, double heading) {
        x = xPos;
        y = yPos;
        theta = heading;
        draw();
    }

    @Override
    public void messageReceived(LCM lcm, String string, LCMDataInputStream stream) {
        try {
            //Get the simple pose data from the input stream
            simplepose_t pose = new simplepose_t(stream);
            System.out.println("Received " + string + " with pos (" + pose.pos[0]
                    + ", " + pose.pos[1] + ")");
        }
        catch (IOException ex) {
            Logger.getLogger(RobotGUI.class.getName()).log(Level.SEVERE, null, ex);
        }

    }

    /**
     * Redraw the current state of the world
     * Includes the floor, the robot, and the robot's path (TODO)
     */
    private void draw() {
        
        //Draw the floor grid
        Buffer floorBuffer = visWorld.getBuffer("floor");
        floorBuffer.addBack(new VzGrid());
        floorBuffer.swap();

        //Draw the robot
        Buffer robotBuffer = visWorld.getBuffer("robot");
        VisChain robotChain = new VisChain(

                //Move to robot position and heading
                LinAlg.translate(x, y),
                LinAlg.rotateZ(theta),

                //Draw robot body
                LinAlg.translate(0, 0, WHEEL_RADIUS + WHEEL_HEIGHT_OFFSET),
                new VzBox(ROBOT_WIDTH, ROBOT_DEPTH, ROBOT_HEIGHT, new VzMesh.Style(Color.blue)),

                //Draw back wheels
                LinAlg.translate(-ROBOT_WIDTH/2 - WHEEL_WIDTH/2, -WHEEL_DEPTH_OFFSET, -WHEEL_HEIGHT_OFFSET),
                LinAlg.rotateY(Math.PI/2),
                new VzCylinder(WHEEL_RADIUS, 0.5, new VzMesh.Style(Color.white)),
                 LinAlg.rotateY(-Math.PI/2),
                LinAlg.translate(ROBOT_WIDTH + WHEEL_WIDTH, 0, 0),
                LinAlg.rotateY(Math.PI/2),
                new VzCylinder(WHEEL_RADIUS, 0.5, new VzMesh.Style(Color.white)),
                LinAlg.rotateY(-Math.PI/2),
                LinAlg.translate(-ROBOT_WIDTH/2 - WHEEL_WIDTH/2, WHEEL_DEPTH_OFFSET, WHEEL_HEIGHT_OFFSET),

                //Draw front wheel
                LinAlg.translate(0, FRONT_OFFSET, -WHEEL_HEIGHT_OFFSET),
                LinAlg.rotateY(Math.PI/2),
                new VzCylinder(WHEEL_RADIUS, 0.5, new VzMesh.Style(Color.green)),
                LinAlg.rotateY(-Math.PI/2),
                LinAlg.translate(0, -FRONT_OFFSET, WHEEL_HEIGHT_OFFSET),

                //Undo robot position and heading
                LinAlg.translate(-x, -y),
                LinAlg.rotateZ(-theta)
                
                );
        robotBuffer.addBack(robotChain);
        robotBuffer.swap();
    }
}
